/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package luft27.comm.mavlink;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Arrays;
import luft27.comm.SerialStream;

/**
 *
 * @author amalikov
 */
public class Protocol {
	public Protocol(SerialStream s) {
		this.s = s;
		seq = 0;
	}
	
	public Message recv() {
		if (!isSTX())
			return null;
		
		byte[] buf = new byte[255 + 7];
		
		if (s.read(buf, 0, 7) != 7)
			return null;

		int len = (short)buf[0] & 0xFF;		
		byte senderId = buf[2];
		byte compId = buf[3];
		byte msgId = buf[4];
		
		if (len > 0 && s.read(buf, 7, len) != len)
			return null;
		
		byte[] payload = Arrays.copyOfRange(buf, 5, len + 5);
		short crc = ByteBuffer.wrap(buf, len + 5, 2).order(ByteOrder.LITTLE_ENDIAN).getShort();
		
		if (getCrc(buf, 0, len + 5, magicCrc.get(msgId)) != crc)
			return null;
		
		return new Message(senderId, compId, msgId, payload);
	}
	
	public void send(Message msg) {
		ByteBuffer bb = ByteBuffer.allocate(msg.payload.length + 8).order(ByteOrder.LITTLE_ENDIAN);
		bb.put(STX);
		bb.put((byte)msg.payload.length);
		bb.put(seq++);
		bb.put(msg.senderId);
		bb.put(msg.compId);
		bb.put(msg.msgId);
		bb.put(msg.payload);
		bb.putShort(getCrc(bb.array(), 1, bb.position(), magicCrc.get(msg.msgId)));
		s.write(bb.array(), 0, bb.position());
	}
	
	private boolean isSTX() {
		return s.read() == STX;
	}
	
	/* 
	 * get CRC for array + magic 
	 */
	private short getCrc(byte[] buf, int from, int to, byte magic) {
		short crc = (short)0xFFFF;
		for (int i = from; i < to; ++i)
			crc = accumulateCrc(crc, buf[i]);
		return accumulateCrc(crc, magic);
	}
	
	/* 
	 * Accumulate one byte of data into the CRC 
	 */
	private short accumulateCrc(short crc, byte value) {
        short tmp = (short)((value ^ (byte)(crc & 0xFF)) & 0xFF);
        tmp ^= (tmp << 4) & 0xFF;
        return (short)(((crc >> 8) & 0xFF) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4));
	}
	
	private final SerialStream s;
	private byte seq;
	private final static byte STX = (byte)0xFE;
	private final static MagicCrc magicCrc = new MagicCrc();
}
